startup_rvc
t = 0:0.05:2;

%----------------------------------------------------------

%   Denavit-Hartenberg parameters that describe the arm

%----------------------------------------------------------

%theta is the variable for this joint. the value provided here
%simply serves as a place holder but theta's value will
%be handled by the link object.
theta = [0, 0, 0];
d = [0.035, 0, 0];
a = [.06, .135, 0];
alpha = [-pi/2, 0, 0];
%Sigma defines wether the joint is revolute (0) or prismatic (1) 
sigma = [0, 0, 0];


%define all links above as link objects
for i = 1:3
   
    L(i) = Link([theta(i), d(i), a(i), alpha(i)]);

end

%serial link all the links

robot_arm = SerialLink(L, 'name', 'Lego Bot');



%---Shift the first and last joint around to increase accuracy of model

%shift the origin of the robot to the playing field
%robot_arm.base = transl(.25, , .05);
%shift the tool up by 17.7cm for the pen
robot_arm.tool = transl(0.177, -0.015, 0);

%find the pose of the end effector
  
%T = robot_arm.fkine(zero);
T1 = [ 1     0    0     .13;  %out radially
      0     1    0     .155;   %left right
      0     0    1      -.035; %up down
      0     0    0     1.0];
  
%T = robot_arm.fkine(zero);
T2 = [ 1     0    0     0.295;  %out radially
      0     1    0     -0.125;   %left right
      0     0    1    -0.035; %up down
      0     0    0     1.0];


%----------------------------------------------------------

%   Cartesian Motion

%----------------------------------------------------------

Ts = ctraj(T1, T2, length(t));

mask = [1 1 1 0 0 0];

qs = robot_arm.ikine(Ts,[0 0 2], mask, 'alpha', .85, 'ilimit', 800);
















%% Start up
h=COM_OpenNXT();
COM_SetDefaultNXT(h);

%% Do your stuff

myMotor             = NXTMotor();   % create class
myMotor.Port        = MOTOR_A;  % same as  myMotors.Port = 'BC';
myMotor.Power       = -80;       % power range from -100 to 100
myMotor.TachoLimit  = 26+30;     % the number of degree of rotation  0 up to 9999 
myMotor.SmoothStart = true;	% motor startup setting
myMotor.ActionAtTachoLimit = 'HoldBrake';   %prevent the motor from moving during stop
myMotor.SpeedRegulation = false;  % no regulation on speed

myMotor1             = NXTMotor();   % create class
myMotor1.Port        = MOTOR_B;  % same as  myMotors.Port = 'BC';
myMotor1.Power       = -80;       % power range from -100 to 100
myMotor1.TachoLimit  = 75+20;     % the number of degree of rotation  0 up to 9999 
myMotor1.SmoothStart = true;	% motor startup setting
myMotor1.ActionAtTachoLimit = 'HoldBrake';   %prevent the motor from moving during stop
myMotor1.SpeedRegulation = false;  % no regulation on speed

myMotor2             = NXTMotor();   % create class
myMotor2.Port        = MOTOR_C;  % same as  myMotors.Port = 'BC';
myMotor2.Power       = 80;       % power range from -100 to 100
myMotor2.TachoLimit  = 116;     % the number of degree of rotation  0 up to 9999 
myMotor2.SmoothStart = true;	% motor startup setting
myMotor2.ActionAtTachoLimit = 'HoldBrake';   %prevent the motor from moving during stop
myMotor2.SpeedRegulation = false;  % no regulation on speed


%path definition
path = [ 1 0 1; 1 0 0; 0 0 0; 0 2 0; 1 2 0;
1 2 1; 0 1 1; 0 1 0; 1 1 0; 1 1 1];




%while 1


myMotor.Stop('off');
myMotor.ResetPosition();
myMotor.SendToNXT();
myMotor.WaitFor();

myMotor1.Stop('off');
myMotor1.ResetPosition();
myMotor1.SendToNXT();
myMotor1.WaitFor();

myMotor2.Stop('off');
myMotor2.ResetPosition();
myMotor2.SendToNXT();
myMotor2.WaitFor();


myMotor1.TachoLimit  = 15
myMotor1.Power       = 80;
myMotor1.Stop('off');
myMotor1.ResetPosition();
myMotor1.SendToNXT();
myMotor1.WaitFor();








%end



    

    
  




%% end of connection
COM_CloseNXT(COM_GetDefaultNXT());